{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {
    "deletable": true,
    "editable": true
   },
   "source": [
    "## Sensor Fusion Algorithms For Autonomous Driving: Part 1 - The Kalman filter and Extended Kalman Filter"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "deletable": true,
    "editable": true
   },
   "source": [
    "### Introduction\n",
    "\n",
    "Tracking of stationary and moving objects is a critical function of Autonomous driving technologies.  Signals from several  sensors, including camera, radar and lidar (Light Detection and Ranging device based on pulsed laser) sensors are combined to estimate the position, veloctiy, trajectory and class of objects i.e. other vehicles and pedestrians.  A good introduction to this topic can be found at:\n",
    "http://www.onenewspage.com/video/20161006/5695999/Mercedes-Benz-presents-the-Sensor-Fusion-at-2016.htm\n",
    "\n",
    "\n",
    "One may question - **why do we need several sensors?** This is because, each sensor provides different types of information about the tracked object position with differing acuracies especially in different weather conditions. For e.g. a lidar based sensor can provide good resolution about the position but can suffer for accuracy in poor weather. On the other hand, the spatial resolution of a radar sensor is relatively poor compared to laser but provides better accuracy in poor weather. Also, unlike a lidar sensor, a radar can provide information about the velocity and bearing of the object. Laser data is also more computaionaly intensive because a laser sends lots of data about each individual laser point of range data, which you hen have to make sense of in your algorithm. The techniques used to merge information from different sensor is called senssor fusion. For reasons discussed earlier, algorithms used in sensor fusion have to deal with temporal, noisy input and generate a probabilistically sound estimate of kinematic state. This blog post covers one of the most common algorithm used in position and tracking estimation called the Kalman filter and its variation called 'Extended Kalman Filter'. In future articles we will cover other techniques such as Unscented Kalman Filters and Particle filters.\n",
    "\n",
    "\n",
    "![Sensor Fusion](./SensorFusion.png)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "deletable": true,
    "editable": true
   },
   "source": [
    "### 1. The Basic Kalman Filter - using Lidar Data\n",
    "The Kalman filter is over 50 years old, but is still one of the most powerful sensor fusion algorithms for smoothing noisy input data and estimating state. It assumes that location variables are gaussian i.e. can be completely parametrized by the mean and the covariance:    $\\color{blue}{X \\sim \\mathcal{N}(\\mu,\\,\\sigma^{2})}$\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "deletable": true,
    "editable": true
   },
   "source": [
    "As information from the sensor flows, the kalman filter uses a series of state prediction and measurement update steps to update its belief about the state of the tracked object. These predict and update steps are described below. \n",
    "\n",
    "![Kalman Filter Cycle](./KalmanCycle.png)\n",
    "\n",
    "#### State Predication: \n",
    "We will use a simplified linear state space model (see https://uk.mathworks.com/help/ident/ug/what-are-state-space-models.html) to illustrate the  workings of the filter. Such a model would be a good fit for laser based sensors that return positional information of the tracked object. The linear state  state of a system at a time t can be estimated from state at time t-1 according to the equation(1):\n",
    "\n",
    "$\\color{blue}{x_{t} = F_{t} x_{t-1} + B_{t} u_{t} + w_{t}}$ *-------equation(1)*  \n",
    "\n",
    "where\n",
    "*    $x_{t}$ is the state vector (postion and velocity) at time t\n",
    "*    $u_{t}$ is the motion vector representing stimulus (steering angle, throttle)\n",
    "*    $F_{t}$ is the state transition matrix \n",
    "*    $B_{t}$ is the control input  \n",
    "*    $w_{t}$ is the noise term for the state vector with zero mean and covariance    $Q_{t}$\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "deletable": true,
    "editable": true
   },
   "source": [
    " The state vector $x_{t}$ holds the position and velocity of the object. Let's represent this state by the vector $x_{t}$=$\\bigl(\\begin{smallmatrix}\n",
    "p \\\\\n",
    "v\n",
    "\\end{smallmatrix} \\bigr)$\n",
    "where\n",
    "* p is the position with dimensions $p_{x}$ and $p_{y}$\n",
    "* v is the velocity with dimensions $v_{x}$ and $v_{y}$"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "deletable": true,
    "editable": true
   },
   "source": [
    "Let us assume that we are tracking a pedestrian moving at constant velocity. The relationship between the predicted state ${x}\\prime$, the previous state and velocity v is given by the kinematic equation:\n",
    "$\\color{blue}{{x}\\prime = {x}  + v\\Delta{t} + 0.5{a}\\Delta{t}^2} + \\nu$\n",
    "\n",
    "where:\n",
    "*    $\\Delta{t}$ is the prediction interval\n",
    "*    \\nu is the process noise with zero mean and covariance Q. The covariance Q surfaces as acceleration noise in the state uncertainty prediction.\n",
    "*    ${a}$ is the acceleration. This will be zero for an object moving at constant velocity\n",
    "\n",
    "In matrix form we can write the above equation as equation(2):\n",
    "\n",
    "$\\color{blue}{{x}\\prime=\\bigl(\\begin{smallmatrix}\n",
    "1&0&\\Delta{t}&0 \\\\\n",
    "0&1&0&\\Delta{t} \\\\\n",
    "0&0&1&0 \\\\\n",
    "0&0&0&1\n",
    "\\end{smallmatrix} \\bigr) \\bigl(\\begin{smallmatrix}\n",
    "p_{x} \\\\\n",
    "p_{y} \\\\\n",
    "v_{x} \\\\\n",
    "v_{y}\n",
    "\\end{smallmatrix} \\bigr)}$  *---------equation(2)*\n",
    "\n",
    "\n",
    "If we compare the above to equation(1), this gives us the state transition matrix as:\n",
    "$\\color{blue}{{F} =\\bigl(\\begin{smallmatrix}\n",
    "1&0&\\Delta{t}&0 \\\\\n",
    "0&1&0&\\Delta{t} \\\\\n",
    "0&0&1&0 \\\\\n",
    "0&0&0&1\n",
    "\\end{smallmatrix} \\bigr)}$"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "deletable": true,
    "editable": true
   },
   "source": [
    "Besides predicting the state, we also need to be able to predict the uncertainty in our belief. This is given by the process covariance matrix P. In the initialisation of the filter, we make assumptions about the how certain we are about our initial state. The variance associated with the prediction is given by: $\\color{blue}{P_{t} = F P_{t-1} F^{T} + Q_{t}}$\n",
    "\n",
    "where ${Q_{t}}$ is the process noise covariance associated with noisy inputs"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "deletable": true,
    "editable": true
   },
   "source": [
    "#### Measurement Update\n",
    "\n",
    "THe next part of the Kalman filter algorithm is to use real measurements $z$ to update the predicted state $x\\prime$ by a  scaling factor (called the Kalman Gain) proportional to the error between the measurment and the the predicted state.\n",
    "\n",
    "This error $y$ (also called the residual) is given by: $\\color{blue}{y = z - H x\\prime}$\n",
    "\n",
    "where ${H}$ is a projection of the state vector in to the measurement space.\n",
    "\n",
    "The Kalman gain is calaculated as :\n",
    "\n",
    "$\\color{blue}{S=HPH^{T}+R}$\n",
    "\n",
    "$\\color{blue}{K=H^{T}PS^{-1}}$\n",
    "\n",
    "$\\color{blue}{x\\prime = x+ Ky}$  *---------------Update the predicted state*\n",
    "\n",
    "$\\color{blue}{P\\prime = (I - KH)P}$  *-----------Update the process uncertainty*\n",
    "\n",
    "\n",
    "where \n",
    "* S is a projection of the process uncertainty into the measurement space\n",
    "* R is the measurement noise\n",
    "\n",
    "You can find the derivation of the measurement update equations at: http://web.mit.edu/kirtley/kirtley/binlustuff/literature/control/Kalman%20filter.pdf\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "collapsed": false,
    "deletable": true,
    "editable": true
   },
   "source": [
    "Enough of theory! Let's try some code to illustrate the basic workings of the KF. Here, we simulate an object whose state is modeled as a 4-dimensional vector ${x}=\\bigl(\\begin{smallmatrix}\n",
    "p_{x} \\\\\n",
    "p_{y} \\\\\n",
    "v_{x} \\\\\n",
    "v_{y}\n",
    "\\end{smallmatrix} \\bigr)$ In our case, the measurement sensor is laser sensor that returns the position data but no velocity information. In order to observe velocity we need to use a Radar sensor. This will be covered in the next section when we discuss Extended Kalman filters. We will start with a set of assumptions:\n",
    "\n",
    "#### Model Assumptions\n",
    "Time interval between predictions dt = 0.1 \n",
    "\n",
    "The initial state vector \n",
    "$\\color{blue}{{x} =\\bigl(\\begin{smallmatrix}\n",
    "4\\\\\n",
    "12 \\\\\n",
    "0 \\\\\n",
    "0\n",
    "\\end{smallmatrix} \\bigr)}$\n",
    "\n",
    "Motion vector\n",
    "$\\color{blue}{{u} =\\bigl(\\begin{smallmatrix}\n",
    "0\\\\\n",
    "0 \\\\\n",
    "0 \\\\\n",
    "0\n",
    "\\end{smallmatrix} \\bigr)}$\n",
    "\n",
    "\n",
    "Initial state uncertainty: we will assume 10 for positions x and y, 100 for the two velocities\n",
    "$\\color{blue}{{P} =\\bigl(\\begin{smallmatrix}\n",
    "10&0&0&0\\\\\n",
    "0&10&0&0 \\\\\n",
    "0&0&100&0 \\\\\n",
    "0&0&0&100\n",
    "\\end{smallmatrix} \\bigr)}$\n",
    "\n",
    "The State transition function\n",
    "$\\color{blue}{{F} =\\bigl(\\begin{smallmatrix}\n",
    "1&0&dt&0 \\\\\n",
    "0&1&0&dt \\\\\n",
    "0&0&1&0 \\\\\n",
    "0&0&0&1\n",
    "\\end{smallmatrix} \\bigr)}$\n",
    "\n",
    "H matrix to map predicted state into measurement space. In the case of a lidar sensor, velocity is not in the measurment space.\n",
    "$\\color{blue}{{H} =\\bigl(\\begin{smallmatrix}\n",
    "1&0&0&0 \\\\\n",
    "0&1&0&0 \n",
    "\\end{smallmatrix} \\bigr)}$\n",
    "\n",
    "Measurement noise\n",
    "$\\color{blue}{{R} =\\bigl(\\begin{smallmatrix}\n",
    "0.1&0 \\\\\n",
    "0&0.1 \n",
    "\\end{smallmatrix} \\bigr)}$\n",
    "\n",
    "\n",
    "Let us simulate a series of (x,y) measurements into the matrix \n",
    "$\\color{blue}{{Z} =\\bigl(\\begin{smallmatrix}\n",
    "5&10 \\\\\n",
    "6&8 \\\\\n",
    "7&6 \\\\\n",
    "8&4 \\\\\n",
    "9&2 \\\\\n",
    "10&2\n",
    "\\end{smallmatrix} \\bigr)}$\n",
    "\n",
    "#### Psuedo code\n",
    "The basic code for the Kalman filter steps is listed below. You can find the code for the basic example at: https://github.com/asterixds/ExtendedKalmanFilter/python\n",
    "\n",
    "```python\n",
    "\n",
    "\"\"\"prediction step\"\"\"\n",
    "def predict(x, P):\n",
    "    x = (F * x) + u \n",
    "    P = F * P * F.transpose() #Acceleration noise Q is assumed to be zero\n",
    "    return x, P\n",
    "\n",
    "\"\"\"measurement update step\"\"\"\n",
    "def update(x, P,z):\n",
    "    # measurement update\n",
    "    Z = matrix([z])\n",
    "    y = Z.transpose() - (H * x)\n",
    "    S = H * P * H.transpose() + R\n",
    "    K = P * H.transpose() * S.inverse()\n",
    "    x = x + (K * y)\n",
    "    P = (I - (K * H)) * P\n",
    "    return x, P\n",
    "```\n",
    "\n",
    "The final step iterates through the measurements and applies the prediction and update steps of the filter as listed above\n",
    "\n",
    "```\n",
    "plot_position_variance(x,P,edgecolor='r')  #plot initial position and covariance in red   \n",
    "for z in measurements:\n",
    "    x,P = predict(x, P)\n",
    "    x,P = update(x, P,z)\n",
    "    plot_position_variance(x,P,edgecolor='b') #plot updates in blue\n",
    "    print(x)\n",
    "    print(P)\n",
    "```\n",
    "\n",
    "![Kalman Filter iterations](./Kalman.png)\n",
    "**Kalman Filter Iterations:** The filter converges to the truth after a few iterations\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "deletable": true,
    "editable": true
   },
   "source": [
    "The above figure illustrate each iteration of the kalman filter for the $p_{x}$ and $p_{y}$  dimensions of the state vector along with the positional covariance. The red circle is a visualisation of our initial process uncertainty. As we go through the sequence of predictions and measurement updates, we beigin to track the object more closely and with less uncertainty (variance). The final state vector $x_{6}$=[11.99, 2.05] is very close to the final measurement value and the positional state variance is also minimal at 0.05"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "deletable": true,
    "editable": true
   },
   "source": [
    "### 2. The Extended Kalman filter - Radar Data"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "deletable": true,
    "editable": true
   },
   "source": [
    "Radar data poses a slightly more difficult challenge. Radar data is returned in Polar co-ordinates which needs to be translated into Cartesian co-ordinates. Radar data consists of 3 components i.e. \n",
    "* $\\rho$ or Range (distance from the origin)\n",
    "* $\\phi$ or bearing (the angle between \n",
    "* $\\rho$ and x, and the $\\dot{\\rho}$ which is the range rate. \n",
    "\n",
    "As there is no $H$ matrix that will map the state vector into the radar measurement space, we need a new function $h(x)$ that will map the state space into the measurement space for the measurement update step. This function is derived by mapping the polar cordinates into the cartesian space and is defined as:\n",
    "$\\color{blue}{h(x) = \\bigl(\\begin{smallmatrix}\n",
    "\\sqrt{{p\\prime_{x}}^2+{p\\prime_{y}}^2} \\\\\n",
    "\\arctan{(p_y/p_x)} \\\\\n",
    "\\frac{p\\prime_{x}v\\prime_{x} + p\\prime_{y}y\\prime_{y}}{\\sqrt{{p\\prime_{x}}^2+{p\\prime_{y}}^2}} \n",
    "\\end{smallmatrix} \\bigr)}$\n",
    "\n",
    "\n",
    "This introduces a non-linearlity which would invalidate the  assumptions of the kalman filter that  the process and measurement noise are Gaussian. The extended kalman filter approximates the nonlinear model by a local linear model and then applies the Kalman filter to this approximation. This local linear approximation is obtained by computing a first order Taylor expansion around the current estimate. The first order approximations are also called the Jacobian Matrix. The derivations of the Jacoboians are a bit involved and we will not be covering these here. However, these are well documented on several internet resources on the topic, but if you want to use these straight off the cuff then you can refer to the implementation code in the github reference below:\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "deletable": true,
    "editable": true
   },
   "source": [
    "### Implementation reference:\n",
    "You can find the code for a C++ impementation of the Kalman filter in the github repository:\n",
    "https://github.com/asterixds/ExtendedKalmanFilter\n",
    "\n",
    "\n",
    "![Test path](./input2.png)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "deletable": true,
    "editable": true
   },
   "source": [
    "### Conclusion\n",
    "So far we have covered some of the fundamental algorithms used in sensor fusion for object tracking. In the next part of this blog post we will look at the Uscented Kalman filter which overcomes the need to use an approximation for the projection. We will also look at a more recent and increasingly popular technique called  Particle filters based on Monte Carlo Integration."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "deletable": true,
    "editable": true
   },
   "source": [
    "## Ness Digital Engineering\n",
    "\n",
    "Ness Digital Engineering works with leading automotive and data technology companies in the  areas of Automotive Safety Product Engineering, Large Scale Fleet Data Solutions, Advanced Driver Assistance Solutions and Location based services. Please reach out to ..."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true,
    "deletable": true,
    "editable": true
   },
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.5.2"
  },
  "widgets": {
   "state": {},
   "version": "1.1.2"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
